//------------------------------------------------------------------------------
//  <copyright file="KinematicChainSelfCollisionChecker.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
//  </copyright>
//------------------------------------------------------------------------------

namespace Microsoft.Robotics.Collision
{
    using System.Collections.Generic;

    using Microsoft.Robotics.Geometry;
    using Microsoft.Robotics.Kinematics;
    using Microsoft.Robotics.Numerics;

    using ShoNS.Array;

    /// <summary>
    /// Check self collision of a given kinematic chain
    /// </summary>
    public class KinematicChainSelfCollisionChecker
    {
        /// <summary>
        /// Check self collision of the kinematic chain
        /// </summary>
        /// <param name="kc">The kinematic chain</param>
        /// <param name="jointValues">Kinematic chain's joint values</param>
        /// <param name="graspedObjectCollisionModel">Collision model of the grasped object in hand</param>
        /// <param name="graspedObjectInEndEffectorTransform">Transform for the object in end effector coordinate system</param>
        /// <returns>Whether the arm is in self collision. True if there is self collision detected.  False otherwise.</returns>
        public static bool CheckSelfCollision(KinematicChain kc, DoubleArray jointValues, GeometryElementGroup3D graspedObjectCollisionModel = null, Matrix4 graspedObjectInEndEffectorTransform = null)
        {
            kc.SetGraspedObjectCollisionModel(graspedObjectCollisionModel);
            List<GeometryElementGroup3D> armCollisionModel = kc.GetTransformedCollisionModels(jointValues, graspedObjectInEndEffectorTransform);

            // check between links of the kinematic chain
            // we ignore neighboring links
            for (int i = 2; i < armCollisionModel.Count; ++i)
            {
                GeometryElementGroup3D currentLinkCollisionModel = armCollisionModel[i];
                if (null == currentLinkCollisionModel)
                {
                    continue;
                }

                for (int j = 0; j <= i - 2; ++j)
                {
                    GeometryElementGroup3D nonNeighboringLinkToCheck = armCollisionModel[j];
                    if (null == nonNeighboringLinkToCheck)
                    {
                        continue;
                    }

                    // evaluate collision between link i and link j
                    double distance = currentLinkCollisionModel.DistanceFrom(nonNeighboringLinkToCheck);

                    if (distance < 0)
                    {
                        return true;
                    }
                }
            }

            return false;
        }
    }
}
